#pragma once
#include <wiringPi.h>
#include <vector>
#include <array>
#include <stdexcept>
#define sk9822_brightness 0x01 // 5-bit brightness,0x00-0x1F

class SK9822 {
public:
    SK9822(int dataPin = 25, int clockPin = 23, int ledCount = 10) {
        if (wiringPiSetup() == -1) {
            throw std::runtime_error("Could not initialize wiringPi!");
        }

        initPins(dataPin, clockPin); // Initialize the data and clock pins

        // Initialize all LEDs to off
        leds.resize(ledCount, {0, 0, 0});
    }

    void setLED(int index, char r, char g, char b) {
        if (index < 0 || index >= leds.size()) {
            throw std::out_of_range("LED index out of range");
        }

        leds[index] = {r, g, b};
    }

    void clear() {
        for (auto& led : leds) {
            led = {0, 0, 0};
        }
        update(); // Update the LED strip to clear all LEDs
    }

    void update() {
         // End frame
        for (int i = 0; i < 4; i++) {
            sendByte(dataPin, clockPin, 0x00);
        }

        for (const auto& led : leds) {
            sendColor(dataPin, clockPin, led[0], led[1], led[2]);
        }

        // End frame
        for (int i = 0; i < 4; i++) {
            sendByte(dataPin, clockPin, 0xFF);
        }
    }

private:
    std::vector<std::array<char, 3>> leds;

    int dataPin;
    int clockPin;

    void initPins(int dataPin, int clockPin) {
        pinMode(dataPin, OUTPUT);
        pinMode(clockPin, OUTPUT);
        digitalWrite(clockPin, LOW);
        digitalWrite(dataPin, LOW);
        this->dataPin = dataPin;
        this->clockPin = clockPin;
    }

    void sendByte(int dataPin, int clockPin, char byte) {
        for (int i = 7; i >= 0; i--) {
            delayMicroseconds(1);
            digitalWrite(dataPin, ((byte >> i) & 1));
            delayMicroseconds(1);
            digitalWrite(clockPin, HIGH);
            delayMicroseconds(1);
            digitalWrite(clockPin, LOW);
            digitalWrite(dataPin, LOW);
        }
    }

    void sendColor(int dataPin, int clockPin, char r, char g, char b) {
        sendByte(dataPin, clockPin, 0xE0 | sk9822_brightness); // 3-bit start code and 5-bit brightness
        sendByte(dataPin, clockPin, b);
        sendByte(dataPin, clockPin, g);
        sendByte(dataPin, clockPin, r);
    }
};